#include "jts_curvature.h"
#include <gts.h>
#include <math.h>

struct JtsSurface_s {
    GtsSurface *surface;
    GtsVertex **vertices;
    int num_vertices;
    int num_triangles;
};

static void revert_bad_faces(GtsSurface *s,
        GtsFace **faces,
        gboolean *faces_ok,
        int num_triangles,
        int which)
{
    int i;
    faces_ok[which] = TRUE;

    for (i = 0; i < num_triangles; ++i) {
        if (!faces_ok[i]) {
            GtsTriangle *t1 = GTS_TRIANGLE(faces[which]);
            GtsTriangle *t2 = GTS_TRIANGLE(faces[i]);
            GtsEdge *edge = gts_triangles_common_edge(t1, t2);
            if (edge != NULL) {
                if (!gts_triangles_are_compatible(t1, t2, edge))
                    gts_triangle_revert(t2);
                revert_bad_faces(s, faces, faces_ok, num_triangles, i);
            }
        }
    }
}


/*
 * Fill a GTS Surface with the half-edge structure corresponding to the
 * given vertices and triangles.
 *
 * Returns the GTS surface in s. You have to free it with gts_object_destroy().
 *
 * Returns the GTS vertices in v. You have to free them with g_free().
 */
JtsSurface *jts_build_surface(JtsVec3f *vertices, int num_vertices,
        JtsTriangle *triangles, int num_triangles)
{
    /* Allocate some memory for temporary vertices storage */
    GtsVertex **gts_v = (GtsVertex**)g_malloc(num_vertices * sizeof(GtsVertex*));
    GtsFace **faces = (GtsFace**)g_malloc(num_triangles * sizeof(GtsFace*));
    gboolean *faces_ok = (gboolean*)g_malloc(num_triangles * sizeof(gboolean));
    GList *edges_list = NULL; /* GList used for edge optimization. */
    int i;
		JtsSurface *s = (JtsSurface*)g_malloc(sizeof(JtsSurface));

    /* Build GTS Vertices */
    for (i = 0; i < num_vertices; ++i) {
        gdouble x = vertices[i][0];
        gdouble y = vertices[i][1];
        gdouble z = vertices[i][2];

        gts_v[i] = gts_vertex_new(gts_vertex_class(), x, y, z);
    }

    s->surface = gts_surface_new(gts_surface_class(),
            gts_face_class(),
            gts_edge_class(),
            gts_vertex_class());
    s->num_vertices = num_vertices;
    s->num_triangles = num_triangles;

    /* Build GTS Edges and Faces */
    for (i = 0; i < num_triangles; ++i) {

        /* Initialize */
        GtsVertex *v1 = gts_v[triangles[i][0]];
        GtsVertex *v2 = gts_v[triangles[i][1]];
        GtsVertex *v3 = gts_v[triangles[i][2]];

        GtsEdge *edge1 = gts_edge_new(gts_edge_class(), v1, v2);
        GtsEdge *edge2 = gts_edge_new(gts_edge_class(), v2, v3);
        GtsEdge *edge3 = gts_edge_new(gts_edge_class(), v3, v1);

        GtsFace *f = gts_face_new(gts_face_class(), edge1, edge2, edge3);
        faces[i] = f;
        faces_ok[i] = FALSE;

        /* Add edges to a list (used for edge merging bellow) */
        edges_list = g_list_append(edges_list, edge1);
        edges_list = g_list_append(edges_list, edge2);
        edges_list = g_list_append(edges_list, edge3);

        /* Store the face */
        gts_surface_add_face(s->surface, f);
    }

    /* Remove duplicate edges */
    edges_list = gts_edges_merge(edges_list);

    /* Revert incompatible triangles */
    revert_bad_faces(s->surface, faces, faces_ok, num_triangles, 0);

    /* Free memory */
    g_free(faces);
    g_free(faces_ok);
    g_list_free(edges_list);

    s->vertices = gts_v;
		return s;
}


void jts_curvatures(JtsSurface *s, JtsCourbure *res)
{
    int i;
    /* gts_surface_print_stats(s->surface, stderr); */

    printf("# \n");
    printf("# Vertex ID/Gaussian/Mean Normal/Mean/Principal/directions\n");
    printf("# \n");
    printf("# Vertex ID: indice starting from 0.\n");
    printf("# Gaussian: Kg [double].\n");
    printf("# Mean Normal: Kh [GtsVector].\n");
    printf("# Mean: Half the magnitude of Kh [double].\n");
    printf("# Principal: K1 [double] K2 [double]\n");
	printf("# directions: e1 [GtsVector] e2 [GtsVector]");
    printf("# \n");

    /* Compute curvatures for all vertices */
    for (i = 0; i < s->num_vertices; ++i) {
        /* Gaussian */
        gdouble Kg;
        gboolean status_Kg;

        /* Mean */
        GtsVector Kh;
        gdouble Kh_mag;
        gboolean status_Kh;

        /* Principal */
        gdouble K1, K2;
		/* directions */
		GtsVector e1, e2;

        status_Kg = gts_vertex_gaussian_curvature(s->vertices[i], s->surface, &Kg);
        if (status_Kg == FALSE)
            printf("# BAD Gaussian Curvature: %d\n", i);

        status_Kh = gts_vertex_mean_curvature_normal(s->vertices[i], s->surface, Kh);
        if (status_Kh == FALSE) {
            printf("# BAD  Mean Curvature Normal: %d\n", i);
        }
        else {
            Kh_mag = 0.5*sqrt(Kh[0]*Kh[0] + Kh[1]*Kh[1] + Kh[2]*Kh[2]);
        }

        gts_vertex_principal_curvatures(Kh_mag, Kg, &K1, &K2);
		
		gts_vertex_principal_directions (s->vertices[i], s->surface, Kh, Kg, e1, e2);

        if (status_Kg && status_Kh) {
            printf("%d / %g / %g,%g,%g / %g / %g %g / %g,%g,%g / %g,%g,%g\n",
                    i, Kg, Kh[0], Kh[1], Kh[2], Kh_mag, K1, K2, e1[0], e1[1], e1[2], e2[0], e2[1], e2[2]);
        }
		
		res[i][0] = Kg;
		res[i][1] = Kh[0];
		res[i][2] = Kh[1];
		res[i][3] = Kh[2];
		res[i][4] = Kh_mag;
		res[i][5] =	K1;
		res[i][6] = K2;
		res[i][7] = e1[0];
		res[i][8] = e1[1];
		res[i][9] = e1[2];
		res[i][10] = e2[0];
		res[i][11] = e2[1];
		res[i][12] = e2[2];
	}
}

	
void jts_double_curvatures(JtsSurface *s, JtsCourbure2 *res)
{
	int i;
    /* gts_surface_print_stats(s->surface, stderr); */

    printf("# \n");
    printf("# Vertex ID/Gaussian/Mean\n");
    printf("# \n");
    printf("# Vertex ID: indice starting from 0.\n");
    printf("# Gaussian: Kg [double].\n");
    printf("# Mean: Half the magnitude of Kh [double].\n");
    printf("# \n");

    /* Compute curvatures for all vertices */
    for (i = 0; i < s->num_vertices; ++i) {
        /* Gaussian */
        gdouble Kg;
        gboolean status_Kg;

        /* Mean */
        GtsVector Kh;
        gdouble Kh_mag;
        gboolean status_Kh;

        status_Kg = gts_vertex_gaussian_curvature(s->vertices[i], s->surface, &Kg);
        if (status_Kg == FALSE)
            printf("# BAD Gaussian Curvature: %d\n", i);

        status_Kh = gts_vertex_mean_curvature_normal(s->vertices[i], s->surface, Kh);
        if (status_Kh == FALSE) {
            printf("# BAD  Mean Curvature Normal: %d\n", i);
        }
        else {
            Kh_mag = 0.5*sqrt(Kh[0]*Kh[0] + Kh[1]*Kh[1] + Kh[2]*Kh[2]);
        }

        if (status_Kg && status_Kh) {
            printf("%d / %g / %g\n", i, Kg, Kh_mag);
        }
		
		res[i][0] = Kg;
		res[i][1] = Kh_mag;
	}
}


void jts_mean_curvatures(JtsSurface *s, double *res)
{
	
	int i;
    /* gts_surface_print_stats(s->surface, stderr); */

    printf("# \n");
    printf("# Vertex ID/Mean\n");
    printf("# \n");
    printf("# Vertex ID: indice starting from 0.\n");
    printf("# Mean: Half the magnitude of Kh [double].\n");
    printf("# \n");

    /* Compute curvatures for all vertices */
    for (i = 0; i < s->num_vertices; ++i) {
        /* Mean */
        GtsVector Kh;
        gdouble Kh_mag;
        gboolean status_Kh;

        status_Kh = gts_vertex_mean_curvature_normal(s->vertices[i], s->surface, Kh);
        if (status_Kh == FALSE) {
            printf("# BAD  Mean Curvature Normal: %d\n", i);
        }
        else {
            Kh_mag = 0.5*sqrt(Kh[0]*Kh[0] + Kh[1]*Kh[1] + Kh[2]*Kh[2]);
        }

        if (status_Kh) {
            printf("%d / %g\n", i, Kh_mag);
        }
		
		res[i] = Kh_mag;
	}
}


void jts_gaussian_curvatures(JtsSurface *s, double *res)
{
	int i;
    /* gts_surface_print_stats(s->surface, stderr); */

    printf("# \n");
    printf("# Vertex ID/Gaussian\n");
    printf("# \n");
    printf("# Vertex ID: indice starting from 0.\n");
    printf("# Gaussian: Kg [double].\n");
    printf("# \n");

    /* Compute curvatures for all vertices */
    for (i = 0; i < s->num_vertices; ++i) {
        /* Gaussian */
        gdouble Kg;
        gboolean status_Kg;

        status_Kg = gts_vertex_gaussian_curvature(s->vertices[i], s->surface, &Kg);
        if (status_Kg == FALSE)
            printf("# BAD Gaussian Curvature: %d\n", i);

        if (status_Kg) {
            printf("%d / %g\n", i, Kg);
        }
		
		res[i] = Kg;
	}
}
